
#include <rtthread.h>
#include <stm32f4xx_hal.h>
#include <attitude_estimate.h>
#include <communication_common.h>
#include <bsp_usart.h>

static int communication_send_msg(uint8_t *msg_buf, uint16_t msg_len)
{
    int rslt = 0;

/* 如果连接了数传电台，使用数传电台发送消息 */
#if COMMUNICATION_WITH_TELEGRAM
    rslt = bsp_telegram_uart_tx(msg_buf, msg_len);
#endif

/* 如果直连计算机，向计算机发送消息，直连方式可能是串口，USB，或者UDP */
#if COMMUNICATION_USE_UART

#endif
#if COMMUNICATION_USE_USB
#endif
#if COMMUNICATION_USE_UDP
#endif

    return rslt;
}

/// @brief 心跳包发送任务
/// @param params
static void heartbeat(void *params)
{
    uint8_t msg_tx_buf[30];
    mavlink_message_t msg_x;

    /* 心跳包 */
    mavlink_msg_heartbeat_pack(
        MAVLINK_UAV_SYSTEM_ID, MAVLINK_UAV_COMPONENT_ID,
        &msg_x, MAV_TYPE_FIXED_WING, MAV_AUTOPILOT_GENERIC,
        MAV_MODE_FLAG_MANUAL_INPUT_ENABLED, 0, MAV_STATE_STANDBY);

    uint16_t msg_len = mavlink_msg_to_send_buffer(msg_tx_buf, &msg_x);
    communication_send_msg(msg_tx_buf, msg_len);
}

/// @brief 发送姿态角任务
/// @param params
static void report_attitude(void *params)
{
    uint8_t msg_tx_buf[50];
    struct attitude_euler euler = {0};
    struct attitude_euler_angular euler_angular = {0};
    mavlink_message_t msg_x;

    get_attitude(&euler, &euler_angular);
    mavlink_msg_attitude_pack(
        MAVLINK_UAV_SYSTEM_ID, MAVLINK_UAV_COMPONENT_ID,
        &msg_x, rt_tick_get(),
        euler.roll, euler.pitch, euler.yaw,
        euler_angular.roll_angular, euler_angular.pitch_angular, euler_angular.yaw_angular);

    uint16_t msg_len = mavlink_msg_to_send_buffer(msg_tx_buf, &msg_x);
    communication_send_msg(msg_tx_buf, msg_len);
}

static void process_mavlink_cmd(mavlink_message_t *msg_x, mavlink_status_t *msg_status)
{
    /* 一个完整的mavlink帧 */
    switch (msg_x->msgid)
    {
    case MAVLINK_MSG_ID_ESC_INFO:
        /* 请求发送当前电调信息 */
        break;

    case MAVLINK_MSG_ID_HOME_POSITION:
        /* 请求发送当前家信息 */
        break;
    default:
        break;
    }
}

/// @brief 通信线程入口函数
/// @param params
static void communication_thread_entry(void *params)
{
#if COMMUNICATION_WITH_RADIO
    sbus_run(); /* 开启sbus监听 */
#endif
#if COMMUNICATION_WITH_TELEGRAM
    telegram_listen(); /* 开启数传监听 */
#endif

    /* 开启不同频率的定时器，用于发送不同消息 */
    rt_timer_t heartbeat_timer = rt_timer_create("heartbeat", heartbeat, RT_NULL, HEARTBEAT_TIMER_PERIOD, RT_TIMER_FLAG_PERIODIC | RT_TIMER_FLAG_SOFT_TIMER);
    if (heartbeat_timer != RT_NULL)
        rt_timer_start(heartbeat_timer);
    else
        rt_kprintf("create heartbeat timer failed\n");

    rt_timer_t report_attitude_timer = rt_timer_create("report_attitude", report_attitude, RT_NULL, REPORT_ATTITUDE_TIMER_PERIOD, RT_TIMER_FLAG_PERIODIC | RT_TIMER_FLAG_SOFT_TIMER);
    if (report_attitude_timer != RT_NULL)
        rt_timer_start(report_attitude_timer);
    else
        rt_kprintf("create report attitude timer failed\n");

#if COMMUNICATION_WITH_TELEGRAM
    mavlink_message_t *msg_x;
    mavlink_status_t *msg_status;
#endif
#if COMMUNICATION_WITH_RADIO
    struct sbus_frame sbus_frame_x;
#endif

    while (1)
    {
#if COMMUNICATION_WITH_TELEGRAM
        /* 监听电台 */
        if (telegram_ready_parse() == RT_EOK)
        {
            if (telegram_read(&msg_x, &msg_status) == 0)
            {
                process_mavlink_cmd(msg_x, msg_status);
                // telegram_print_frame(msg_x);
            }
            telegram_listen(); /* 重新开始监听电台 */
        }
#endif

#if COMMUNICATION_USE_UART
#elif COMMUNICATION_USE_USB
#elif COMMUNICATION_USE_UDP
#endif

#if COMMUNICATION_WITH_RADIO
        /* 监听SBUS帧 */
        if (sbus_ready_parse() == RT_EOK)
        {
            sbus_resovle(&sbus_frame_x);
            sbus_run(); /* 重新开始监听sbus帧 */
        }
#endif
        rt_thread_mdelay(10);
    }
}

int start_communication_thread(void)
{
    bsp_sbus_uart_init();
    // bsp_telegram_uart_init();

    rt_thread_t communication_thread = rt_thread_create(
        "communication_thread",
        communication_thread_entry,
        RT_NULL,
        COMMUNICATION_THREAD_STACK_SIZE,
        COMMUNICATION_THREAD_PRIORITY,
        COMMUNICATION_THREAD_TICK);

    if (!communication_thread)
    {
        rt_kprintf("launch communication thread failed\n");
        return -1;
    }

    rt_thread_startup(communication_thread);
    return 0;
}